//#include "ros/ros.h"
//#include <std_msgs/String.h>
//#include <std_msgs/Empty.h>
//#include <geometry_msgs/PoseStamped.h>
//#include <geometry_msgs/TwistStamped.h>
//#include <geometry_msgs/Point.h>
//#include <geometry_msgs/Pose.h>
//#include <geometry_msgs/PoseWithCovarianceStamped.h>
//
//#include <iostream>
//#include <fstream>
//#include <sstream>
//#include <queue>
//#include <opencv2/imgcodecs.hpp>
//#include <opencv2/imgproc.hpp>
//#include <opencv2/videoio.hpp>
//#include <opencv2/highgui.hpp>
//#include <opencv2/video.hpp>
//#include <opencv2/video/background_segm.hpp>
//#include "opencv2/bgsegm.hpp"
//#include <vector>
//#include <opencv2/viz.hpp>
//#include <opencv2/core/utility.hpp>
//#include <opencv2/core.hpp>
//#include <opencv2/calib3d.hpp>
//
//#define PENGZHANG 1
//#define THETA_X (28*3.141592653589/180)
//#define THETA_Y 0
//#define THETA_Z 3.141592653589
//
//#define CAP_INDEX 0
////"/media/lijiayi/OS/Users/69067/Pictures/Camera Roll/WIN_20190326_21_32_49_Pro.mp4"
//
//using namespace cv;
//using namespace std;
//using namespace ros;
//
//ofstream ViconFile, CameraFile;
//Time start_time;
//
//
//void ViconPosCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//    Duration dur = Time::now()-start_time;
//    ViconFile << dur.toSec() << "," << msg->pose.position.x << ","<< msg->pose.position.y << ","<< msg->pose.position.z << endl;
//}
//
//int main(int argc, char* argv[]) {
//    init(argc,argv, "drone_pnp_node");
//    NodeHandle n;
//    Subscriber pos_sub=n.subscribe("/mocap/pose",1,ViconPosCallback);
//    Publisher pos_pub=n.advertise<geometry_msgs::PoseStamped>("/cam/pose", 10);
//    Publisher vel_pub=n.advertise<geometry_msgs::TwistStamped>("/cam/vel", 10);
//    Rate loop_rate(40)
//
//
//
#include "ros/ros.h"
#include "ros/package.h"

//#include <message_filters/subscriber.h>
//#include <message_filters/time_synchronizer.h>
//#include <message_filters/sync_policies/approximate_time.h>
#include <thread>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <opencv2/highgui.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/core.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

//#include <drone_pnp/stopRec.h>

using namespace std;
using namespace ros;
using namespace cv;
using namespace pcl;
using namespace sensor_msgs;
//using namespace message_filters;

bool start_flag = false, end_flag = false;
ofstream of_vicon, of_cam;
ifstream vicon_if, cam_if;
Time start_time;
//Mat depth_img;
//bool stop = false;
//
//bool cb_srv_stopRec(drone_pnp::stopRec::Request  &req, drone_pnp::stopRec::Response &res){
//    stop = req.stop;
//}

//void callback(const geometry_msgs::PoseStampedConstPtr& cam_pos, const geometry_msgs::PoseStampedConstPtr& vicon_pos)
//{
//    std::cout << "============" << endl << cam_pos->pose.position << vicon_pos->pose.position << endl;
//        cali_data << cam_pos->header.stamp.toSec() << "," << cam_pos->pose.position.x
//        << "," << cam_pos->pose.position.y<< "," << cam_pos->pose.position.z << ","
//             << vicon_pos->header.stamp.toSec() << "," << vicon_pos->pose.position.x
//             << "," << vicon_pos->pose.position.y<< "," << vicon_pos->pose.position.z << endl;
//}


void vicon_cb(const geometry_msgs::PoseStampedConstPtr &msg_pos) {
    Duration dur = Time::now() - start_time;
    of_vicon << dur.toSec() << "," << msg_pos->pose.position.x
             << "," << msg_pos->pose.position.y << "," << msg_pos->pose.position.z << endl;
}

void cam_cb(const geometry_msgs::PoseStampedConstPtr &msg_pos) {
    Duration dur = Time::now() - start_time;
    of_cam << dur.toSec() << "," << msg_pos->pose.position.x
           << "," << msg_pos->pose.position.y << "," << msg_pos->pose.position.z << endl;
}


int main(int argc, char **argv) {
    ros::init(argc, argv, "trans_cali");
    ros::NodeHandle nh;
    ros::Rate loop_rate(30);
    int aaaa = 0;
    cout << "Press s to start record data to file for 5 seconds," << endl
         << "Press r to read from file directly." << endl
         << "Press e to end" << endl;
    int key = getchar();
    if (key == 's') {
        start_flag = true;
        end_flag = false;
    } else if (key == 'r')
        end_flag = true;
    else if (key == 'e')
        return 0;

    //    ros::ServiceServer service = nh.advertiseService("stop_img", cb_srv_stopRec);

//    message_filters::Subscriber<geometry_msgs::PoseStamped> cam_sub(nh, "/cam/pose", 100);
//    message_filters::Subscriber<geometry_msgs::PoseStamped> vicon_sub(nh, "/mocap/pose", 100);
//1
//    typedef sync_policies::ApproximateTime<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped> MySyncPolicy;
//    Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000), cam_sub, vicon_sub);
//    sync.registerCallback(boost::bind(&callback, _1, _2));
//2
//    TimeSynchronizer<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped > sync(cam_sub, vicon_sub, 100);
//    sync.registerCallback(boost::bind(&callback, _1, _2));

    Subscriber vicon_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mocap/pose", 10, vicon_cb);
    Subscriber cam_sub = nh.subscribe<geometry_msgs::PoseStamped>("/cam_raw/pose", 10, cam_cb);


    if (start_flag && !end_flag) {
        std::string pkg_path = ros::package::getPath("drone_pnp");
        of_vicon.open(pkg_path + "/data/cali_vicon_data.csv");
        of_cam.open(pkg_path + "/data/cali_cam_data.csv");
        if (!of_cam.is_open() || !of_vicon.is_open()) {
            ROS_ERROR("%s",("Can't write data file to:  " + pkg_path + "/data/").c_str());
            return 0;
        }
        start_time = Time::now();
        Time current_time;
        while (ros::ok() && aaaa <= 30 * 5) {
            cout << aaaa << endl;
            aaaa++;
            ros::spinOnce();
            loop_rate.sleep();
        }
        end_flag = true;
        of_vicon.close();
        of_cam.close();
    }
    if (end_flag) {
        std::string pkg_path = ros::package::getPath("drone_pnp");
//        cout << ros::package::getPath("drone_pnp")<<endl;
        vicon_if.open(pkg_path + "/data/cali_vicon_data.csv");
        cam_if.open(pkg_path + "/data/cali_cam_data.csv");
        if (!vicon_if.is_open() || !cam_if.is_open()) {
            ROS_ERROR("%s",("Can't read data file from:  " + pkg_path + "/data/").c_str());
            return 0;
        }

        string lineStr;
        vector<vector<string>> vicon_strArray, cam_strArray;

        while (getline(vicon_if, lineStr)) {
//            // 打印整行字符串
//            cout << lineStr << endl;
            // 存成二维表结构
            stringstream ss(lineStr);
            string str;
            vector<string> lineArray;
            // 按照逗号分隔
            while (getline(ss, str, ','))
                lineArray.push_back(str);
            vicon_strArray.push_back(lineArray);
        }

        while (getline(cam_if, lineStr)) {
//            // 打印整行字符串
//            cout << lineStr << endl;
            // 存成二维表结构
            stringstream ss(lineStr);
            string str;
            vector<string> lineArray;
            // 按照逗号分隔
            while (getline(ss, str, ','))
                lineArray.push_back(str);
            cam_strArray.push_back(lineArray);
        }

        if (vicon_strArray.size() < cam_strArray.size() - 1 || vicon_strArray.size() > cam_strArray.size() + 1) {
            ROS_ERROR("Point amount doesn't equal.");
            cout << vicon_strArray.size() << cam_strArray.size() << endl;
            return 0;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

        cloud_in->width = cam_strArray.size();
        cloud_out->width = vicon_strArray.size();
        cloud_in->height = 1;
        cloud_out->height = 1;
        cloud_in->points.resize(cloud_in->width * cloud_in->height);
        cloud_out->points.resize(cloud_in->width * cloud_in->height);
        for (size_t i = 0; i < cloud_in->points.size(); i++) {
            cloud_in->points[i] = pcl::PointXYZ(std::stof(cam_strArray[i][1]), std::stof(cam_strArray[i][2]),
                                                std::stof(cam_strArray[i][3]));
            cout << "Loaded point:" << cam_strArray[i][1] << cam_strArray[i][2] << cam_strArray[i][3] << endl;

            cloud_out->points[i] = pcl::PointXYZ(std::stof(vicon_strArray[i][1]), std::stof(vicon_strArray[i][2]),
                                                 std::stof(vicon_strArray[i][3]));
            cout << "Loaded point:" << vicon_strArray[i][1] << vicon_strArray[i][2] << vicon_strArray[i][3] << endl;
        }
        std::cout << "Saved " << cloud_in->points.size() << " data points to input" << std::endl;


        pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
        viewer->setBackgroundColor(0, 0, 0);
        //cam cloud
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green_color(cloud_in, 0, 255, 0);
        viewer->addPointCloud<pcl::PointXYZ>(cloud_in, green_color, "cam");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cam");
        //vicon cloud
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color(cloud_in, 255, 0, 0);
        viewer->addPointCloud<pcl::PointXYZ>(cloud_out, red_color, "vicon");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "vicon");

        viewer->addCoordinateSystem(1.0);
        viewer->initCameraParameters();
        viewer->spinOnce();


//        IterativeClosestPoint<PointXYZ, PointXYZ> icp;
//         // Set the input source and target
//        icp.setInputSource (cloud_in);
//        icp.setInputTarget (cloud_out);
//        // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
//        icp.setMaxCorrespondenceDistance (0.05);
//        // Set the maximum number of iterations (criterion 1)
//        icp.setMaximumIterations (50);
//        // Set the transformation epsilon (criterion 2)
//        icp.setTransformationEpsilon (1e-8);
//        // Set the euclidean distance difference epsilon (criterion 3)
//        icp.setEuclideanFitnessEpsilon (1);
//        // Perform the alignment
//        pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
//        icp.align(*Final);
//        std::cout << "has converged:" << icp.hasConverged() << " score: " <<
//                  icp.getFitnessScore() << std::endl;
//        // Obtain the transformation that aligned cloud_source to cloud_source_registered
//        Eigen::Matrix4f transformation = icp.getFinalTransformation ();

        pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> tfe;
//        pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 mtx;
        Eigen::Matrix4f transformation;
        tfe.estimateRigidTransformation(*cloud_in, *cloud_out, transformation);

        pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*cloud_in, *Final, transformation);

        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setInputSource(cloud_in);
        icp.setInputTarget(cloud_out);
        icp.setMaximumIterations(50);
        icp.align(*Final, transformation);
        std::cout << "has converged:" << ((icp.hasConverged() == 1) ? "True" : "False") << " score: " <<
                  icp.getFitnessScore() << std::endl;
        std::cout << icp.getFinalTransformation() << std::endl;

        //final cloud
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color(cloud_in, 0, 0, 255);
        viewer->addPointCloud<pcl::PointXYZ>(Final, blue_color, "final");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "final");


        while (!viewer->wasStopped()) {
            viewer->spinOnce(100);
        }
        ofstream mtx;
        mtx.open(pkg_path + "/data/transformation_matrix.txt");
        Eigen::Matrix4f finalmtx;
        finalmtx = icp.getFinalTransformation();
        if (mtx.is_open())
            mtx << finalmtx;
        else
            ROS_ERROR("Can't write matrix file.");
        mtx.close();
        cout << "The transformation matrix has been saved." << endl;

    }

    return 0;
}
